/*
 * ReadAnalogToSerial.c
 *
 * Created: 16/5/2554 11:01AM
 *  Author: Sathaporn C.
 *
 *	Credit PID:http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853
 *   	   PID:http://www.dprg.org/tutorials/2003-10a/motorcontrol.pdf
 */ 

#include <stdio.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#define F_CPU 20000000 

#include "../EzyBot/EzybotUsart.h"
//#include "../EzyBot/EzybotSensor.h"
#include <util/delay.h>

#define sbi(var, mask)   ((var) |= (1 << mask))
#define cbi(var, mask)   ((var) &= ~(1 << mask))

#define BACKWARD						(1)
#define FORWARD							(0)

#define	encodeLeft1		read_ADC(4)
#define	encodeLeft2		read_ADC(5)
#define	encodeRight1	read_ADC(6)
#define	encodeRight2	read_ADC(7)

volatile unsigned char encodeLeft[2];
volatile unsigned char encodeRight[2];
volatile  int LeftWheel[2];
volatile  int RightWheel[2];

int speed_req = 300;                            // speed (Set Point)
int speed_act = 0;                              // speed (actual value)
int PWM_val = 0;                                // (25% = 64; 50% = 127; 75% = 191; 100% = 255)
float Kp =   .3;                                // PID proportional control Gain
float Ki =	 .1;
float Kd =   .2;                                // PID Derivitave control gain


int RightWheelState[2];
int LeftWheelState[2];


void init_ADC(void)
{
	ADCSRA = (1 << 7)|(0 << 5);	
	ADCSRA |= (0 << ADPS2)|(1 << ADPS1)|(1 << ADPS0);
}

unsigned int Data;
unsigned char read_ADC(unsigned char adc_Ch)
{
	Data=0;
	//for(int i=0;i<63;i++)
	//{
		ADMUX = (0 << REFS1)|(1 << REFS0)|(adc_Ch);
		ADCSRA |= 0x40;							// Start ADC
		while ((ADCSRA & 0x10)==0);				// Wait for the AD conversion to complete
		ADCSRA |= 0x10;
		Data=(ADCW>>2);
	//}
	return (unsigned char)(Data); //divide by 4, 8-bit Resolution
}

void ReadEncoder()
{
	encodeLeft[0] = read_ADC(4);
	encodeLeft[1] = read_ADC(5);
	encodeRight[0] = read_ADC(6);
	encodeRight[1] = read_ADC(7);
}

void InitPWM()
	{

		TCCR0A = (1<<COM0A1) |
				 (0<<COM0A0) |
				 (1<<COM0B1) |
				 (0<<COM0B0) |
				 (1<<WGM01)	 |
				 (1<<WGM00);
		TCCR0B = (0<<WGM02)  |
				 (0<<CS02)   |
				 (1<<CS01)	 |
				 (1<<CS00);

		OCR0A = 0x00;
		OCR0B = 0x00;

		PORTD &= ~(1<<PD5)&~(1<<PD6);
		PORTB &= ~(1<<PB3)&~(1<<PB4);
		
		DDRD |= (1 << PORTD5) | (1 << PORTD6);
	    DDRB |= (1 << PORTB3) | (1 << PORTB4);
	}
	void UpdatePWM(unsigned char LeftPwmVal,unsigned char LeftDirection,unsigned char RightPwmVal,unsigned char RightDirection)
	{				
		if(LeftPwmVal>100)
		{
			LeftPwmVal=255;
		}
		else
		{
			LeftPwmVal=LeftPwmVal*(255/100);
		}
		if(RightPwmVal>100)
		{
			RightPwmVal=255;
		}
		else
		{
			RightPwmVal=RightPwmVal*(255/100);
		}

		if(LeftDirection>0)
		{
			OCR0B = 255-LeftPwmVal;
			PORTB |= (1<<PB3);
		}
		else
		{
			OCR0B = LeftPwmVal;			
			PORTB &= ~(1<<PB3);
		}

		if(RightDirection>0)
		{
			OCR0A = 255-RightPwmVal;
			PORTB |= (1<<PB4);			
		}
		else
		{
			OCR0A = RightPwmVal;
			PORTB &= ~(1<<PB4);
		}
	}
	void StopPwm()
	{
		
		TCCR0A = (0<<COM0A1) |
				 (0<<COM0A0) |
				 (0<<COM0B1) |
				 (0<<COM0B0) |
				 (0<<WGM01)	 |
				 (0<<WGM00);
		TCCR0B = (0<<WGM02)  |
				 (0<<CS02)   |
				 (0<<CS01)	 |
				 (0<<CS00);
		
		PORTD |= (1<<PD6)|(1<<PD5);						 
		PORTB |= (1<<PB4)|(1<<PB3);						 
	}
	void StartPwm()
	{		
		InitPWM();
	}

	int constrain(int value)
	{
		if(value < 0){
			return 0;
		}
		else if(value > 100){
			return 100;
		}
		else{
			return value;
		}
	}

	int updatePid(int command, int targetValue, int currentValue)   // compute PWM value			
	{            													// PID correction
		float pidTerm = 0;                                                            
		float error =  0;    
		float integral = 0;  
		float derivative = 0;                            
		static float last_error = 0;                            
		
		error = (targetValue) - (currentValue);
		integral = (integral) + (error);
		derivative = (error - last_error);
		pidTerm = (Kp * error) + (Ki * integral) + (Kd * derivative);                       
		last_error = error;

		//printf("&&&& %d\n", ((int)command + (int)pidTerm));
		//while(1);

		return constrain((int)command + (int)pidTerm);
	}

int main (void)
{
	init_ADC();
	USARTinit();
	printf("Hello\n");


	DDRC |= (1<<PC2);    // PC2 as output
	DDRB |= (1<<PB2);    // PC2 as output

	StartPwm();
	
	UpdatePWM(0,FORWARD,0,FORWARD);
	
	//while(1)// loop check value
	//{
	//	ReadEncoder();	
	//	printf("%d\n", encodeLeft[0]);
	//	_delay_ms(200);
	//}
    while(1)
    {
		sbi(PORTC, 2);
		sbi(PORTB, 2);

		TCCR1A = 0;      // normal mode
		TCCR1B = 0x05;   // 5 is pre1024
		TCCR1C = 0; 
	
	
		// example for use.
		//ReadEncoder();
		//UARTString("ReadEncode...\r\n");
		UARTString("encodeL1\r\n");
		UARTString("encodeL2\r\n");
		UARTString("waiting 1.2 sec.\r\n");
		UARTString("1.2..\r\n");
		_delay_ms(200);
		UARTString("1..\r\n");
		_delay_ms(1000);
		UARTString("OK..\r\n");

		//unsigned char sampling[50];
		//unsigned int samplingTime[50];
		//unsigned char sampling1[50];
		//unsigned int samplingTime1[50];
		//memset(sampling, 0, sizeof(sampling));
		unsigned int countL[2];

		TCNT1 = 0;	
		//_delay_ms(10);
		//printf(">>>>>%u\n", TCNT1);
	
		static volatile char countFlag = 1;
		countL[0] = 0;
		countL[1] = 0;
		UpdatePWM(0, FORWARD, 0, FORWARD);
		float currentSpeed;
		//int currentPWM = 0;
		int countPrintSpeed = 0;
		int timeUpdatePID = 0;
		PWM_val = 0;
		while(1)
		{
			//ReadEncoder();
			encodeLeft[0] = read_ADC(4);
			encodeLeft[1] = read_ADC(5);
			//debug value analog IR.
			/*
			printf("%d\n", encodeLeft[1]);
			_delay_ms(100);	
			*/

			//bafore start sampling pulse encode waiting just minute
			//cuz "count>2" value's 2 don't worry this value 
			if(countL[0]>2 && countFlag==1){
				TCNT1 = 0;
				countL[0] = 0;
				countL[1] = 0;
				countFlag = 0;
				LeftWheel[0] = 0;
				LeftWheel[1] = 0;
				countPrintSpeed = 0;
			}
				
			//counting wheel encoder, this wheel have through 3rd zone, 
			//in this zone value is mininum value"(encodeLeft[0] < 99)"
			//so that 1 round of wheel count through 4 time
			if(encodeLeft[0] < 85){	//IR through black wheel
				if(LeftWheelState[0] == 1)
				{
					LeftWheel[0]++;
					countPrintSpeed++;
					//sampling[countL[0]] = 1;
					//samplingTime[countL[0]++] = TCNT1;
					//UARTString("R0 1\r\n");
					//printf("1 %d\n", encodeLeft[0]);

					//printf("%d\n", countPrintSpeed);	//check wheel 1 round How many round code.
				}
				LeftWheelState[0] = 0;					
			}
			if(encodeLeft[0] > 140){	//wheel encode close sensor
				if(LeftWheelState[0] == 0)
				{
					//LeftWheel[0]++;
					//countPrintSpeed++;
					//sampling[countL[0]] = 0;
					//samplingTime[countL[0]++] = TCNT1;
					//UARTString("R0 0\r\n");
					//printf("0 %d\n", encodeLeft[0]);
						
					//printf("%d\n", LeftWheel[0]);	//check wheel 1 round How many round code.
				}
				LeftWheelState[0] = 1;	
			}
			//////////////////////////////////////////////////////////////
			/*
			if(encodeLeft[1] < 85){	//IR through black wheel
				if(LeftWheelState[1] == 1)
				{
					//LeftWheel[1]++;
					//countPrintSpeed++;
					//sampling1[countL[1]] = 1;
					//samplingTime1[countL[1]++] = TCNT1;
					//UARTString("R1 1\r\n");
					//printf("1 %d\n", encodeLeft[1]);

					//printf("%d\n", RightWheel);	//check wheel 1 round How many round code.
				}
				LeftWheelState[1] = 0;					
			}
			if(encodeLeft[1] > 140){	//wheel encode close sensor
				if(LeftWheelState[1] == 0)
				{
					//LeftWheel[1]++;
					//countPrintSpeed++;
					//sampling1[countL[1]] = 0;
					//samplingTime1[countL[1]++] = TCNT1;
					//UARTString("R1 0\r\n");
					//printf("0 %d\n", encodeLeft[1]);
						
					//printf("%d\n", RightWheel);	//check wheel 1 round How many round code.
				}
				LeftWheelState[1] = 1;	
			}	
			*/
			//count for round of wheel after gearbox.
			//340, 4 is count of one round of encode wheel 
			/*
			if(countPrintSpeed == 340){
				UpdatePWM(0,FORWARD,0,FORWARD);
				UpdatePWM(100,BACKWARD,0,FORWARD);
				_delay_ms(50);
				UpdatePWM(0,BACKWARD,0,FORWARD);

				_delay_ms(1500);				
				UpdatePWM(30,FORWARD,0,FORWARD);
				countPrintSpeed = 0;
			}
			*/
			
			
			 
			//Speed wheel rpm/min.
			
			if(LeftWheel[0] == 4){
				currentSpeed = (((float)(1))/((float)TCNT1*5.12e-5));
				//printf("round/time >> %u\n", (int)currentSpeed);
				//printf("time 1 round of wheel >> %u\n", TCNT1);
				timeUpdatePID += TCNT1;
				TCNT1 = 0;
				countL[0] = 0;
				countL[1] = 0;
				countFlag = 0;
				LeftWheel[0] = 0;	
				//printf("ok!!!.\n");
			}
			

			//Update PID every 100ms.
			//1953 is 100ms/5.12e-5 : pre1024, 20Mhz
			
			if(timeUpdatePID > 1953 || TCNT1 > 1953){
				speed_act = currentSpeed;
				speed_req = 130;
				PWM_val = updatePid(PWM_val, speed_req, speed_act); 
				//printf("speed_req : %u\n", speed_req);
				//printf("speed_act : %u\n", speed_act);				
				UpdatePWM(PWM_val, FORWARD, 0, FORWARD);
				timeUpdatePID = 0;
			}
			

			//if count compleat 1 round of wheel print current speed motor;
			
			if(countPrintSpeed == 340){
				printf("rpm >> %u\n", (int)currentSpeed);
				countPrintSpeed = 0;
			}			

			//print sampling for analysis in grapht excell
			/*
			if(countL[0] > 49){
				countL[0] = 0;				
				printf("End of sampling\n");
				printf("there are value 100 value\n");
				
				printf("Encoder Left0\n");
				while(1){					
					printf("%u\t%d\n", samplingTime[countL[0]], sampling[countL[0]]);
					printf("%u\t%d\n", samplingTime[countL[0]+1], sampling[countL[0]]);
					countL[0]++;
					if(countL[0] > 48){
						printf("end of Left0..... :D\n");
						break;
					}
					_delay_ms(10);
				}
				printf("Encoder Left1\n");
				countL[1] = 0;
				while(1){					
					printf("%u\t%d\n", samplingTime1[countL[1]], sampling1[countL[1]]);
					printf("%u\t%d\n", samplingTime1[countL[1]+1], sampling1[countL[1]]);
					countL[1]++;
					if(countL[1] > 48){
						printf("end..... :D\n");
						while(1){
							_delay_ms(10);
						}
					}
					_delay_ms(10);
				}	
			}	
			*/
		}

		//capture encode.
		printf("Sampling Encode 3 sec...\n");
		char encodeL1[100];
		char encodeL2[100];

		memset(encodeL1, 0, sizeof(encodeL1));
		memset(encodeL2, 0, sizeof(encodeL2));
		
	

		printf("end of sampling\n");
		while(1);
		//end of capture encode

	
		cbi(PORTC, 2);
		cbi(PORTB, 2);
		_delay_ms(500);
    }
   
    return(0);
}

